/*
 *  Mikrokopter Driver
 *  Copyright (C) 2012, CYPHY lab
 *  Inkyu Sa <i.sa@qut.edu.au>
 *
 *  http://wiki.qut.edu.au/display/cyphy
 *
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef MIKO_DRIVER_H
#define MIKO_DRIVER_H

// %Tag(FULLTEXT)%
// %Tag(ROS_HEADER)%
#include "ros/ros.h"
// %EndTag(ROS_HEADER)%
// %Tag(MSG_HEADER)%
#include "std_msgs/String.h"
#include "std_msgs/Int32MultiArray.h"
#include "std_msgs/Float32MultiArray.h"
#include "geometry_msgs/Vector3.h"
#include "sensor_msgs/Imu.h"
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/TwistStamped.h>
// %EndTag(MSG_HEADER)%

#include <sstream>
#include <time.h>
#include <stdio.h>
#include <iostream>
#include <string.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>


#include <stdlib.h>
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <pthread.h>
#include <cstdlib>


#include <cstdlib>
#include <time.h>
#include <errno.h>
#include <bitset>
#include <stdarg.h>

#include "imu/mikoCmd.h"

#define BAUDRATE B57600 //
#define IMU_PORT "/dev/ttyUSB0"
//#define IMU_PORT "/dev/ttyUSB1"
#define FALSE 0
#define TRUE 1
#define MAX_THROTTLE 180
#define PI 3.141592653589793238462643383279
#define deg2rad(x) (x*0.0174532925f)
#define rad2deg(x) (x*57.2957795f)
#define g 9.81   //9.8 m/s^2

using namespace std;

FILE* fid;

volatile int STOP=FALSE;
struct termios oldtio,newtio;
time_t t_start, t_end, t1_start;
string str("");
int pbuf[31];
int pre_pbuf[9]={0,0,0,0,0,0,0,0,0};
int bias_pbuf[31]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
int seqs=0;
int time_cnt=0;
double gyro_scale=4.0417;

void ROS_IMU_Msgs();
void ROS_Stick_Msgs();
std_msgs::Float32MultiArray msg;
std_msgs::Float32MultiArray msg_imu;
sensor_msgs::Imu ros_imu_msg;
geometry_msgs::TwistStamped msg_stick_cmd;

int fd=0;

#define FC_ADDRESS 1
char g_txd_buffer[1000];

#endif

